

#include "LinearMath/btTransform.h"

#include "HalfHumanCreature.h"

namespace jcrada {

    HalfHumanCreature::HalfHumanCreature( int hidden_layers, int neurons_per_layer) :
    Creature( BODY_PARTS, CONSTRAINTS, hidden_layers, neurons_per_layer) {
        setName("HalfHumanCreature");
    }

    HalfHumanCreature::~HalfHumanCreature( ) {

    }

    void HalfHumanCreature::set( const btTransform& at ) {
        setBodyParts();
        setConstraints();

        btTransform transform;
        double max_height = 0.0;
        max_height += getBodyPart(B_HEAD).getSizeY() + getBodyPart(B_THORAX).getSizeY() ;//+ getBody(B_THORAX).getSizeR() + getBody(B_THORAX).getSizeR();

        double current_height = max_height;
        //HEAD
        current_height -= getBodyPart(B_HEAD).getSizeR();
        transform.setIdentity();
        transform.setOrigin(btVector3(0.0, current_height, 0.0));
        getBodyPart(B_HEAD).set(at * transform);
        current_height -= getBodyPart(B_HEAD).getSizeR();

        //THORAX
        current_height -= getBodyPart(B_THORAX).getSizeY() / 2.0;
//        current_height -= getBody(B_THORAX).getSizeR();

        transform.setIdentity();
        transform.setOrigin(btVector3(0.0, current_height, 0.0));
        getBodyPart(B_THORAX).set(at * transform);
        current_height -= getBodyPart(B_THORAX).getSizeY() / 2.0;

        double current_width = 0.0;

        current_height = max_height - getBodyPart(B_HEAD).getSizeY();
        current_width = getBodyPart(B_THORAX).getSizeX() / 2.0;
        //UPPERARMS
//        current_height -= getBody(B_THORAX).getSizeR();
        current_height -= getBodyPart(B_LUPPERARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LUPPERARM).getSizeR();
        current_width += getBodyPart(B_LUPPERARM).getSizeX() / 2.0;

        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LUPPERARM).set(at * transform);
        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RUPPERARM).set(at * transform);


        current_height -= getBodyPart(B_LUPPERARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LUPPERARM).getSizeR();
        //        //FOREARMS
        current_height -= getBodyPart(B_LFOREARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LFOREARM).getSizeR();
        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LFOREARM).set(at * transform);

        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RFOREARM).set(at * transform);
        current_height -= getBodyPart(B_RFOREARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LFOREARM).getSizeR();

        for (int i = 0; i < getNumberOfConstraints(); ++i) {
            getConstraint(i).set(*this);
        }
        calculateMaxTorque();
    }

    void HalfHumanCreature::setBodyParts( ) {
        getBodyPart(B_HEAD).setId(B_HEAD);
        getBodyPart(B_HEAD).setName("Head");
        getBodyPart(B_HEAD).setParentId(B_THORAX);
        getBodyPart(B_HEAD).setShape(BodyPart::S_SPHERE);
        getBodyPart(B_HEAD).setSize(0.25, 0.25, 0.25);
        getBodyPart(B_HEAD).setMass(4.4);

        getBodyPart(B_THORAX).setId(B_THORAX);
        getBodyPart(B_THORAX).setName("Thorax");
        getBodyPart(B_THORAX).setParentId(Creature::B_NONE);
        getBodyPart(B_THORAX).setShape(BodyPart::S_BOX);
        getBodyPart(B_THORAX).setSize(0.305, 0.829, 0.225);
        getBodyPart(B_THORAX).setMass(33.4);

        getBodyPart(B_LUPPERARM).setId(B_LUPPERARM);
        getBodyPart(B_LUPPERARM).setName("LeftUpperArm");
        getBodyPart(B_LUPPERARM).setParentId(B_THORAX);
        getBodyPart(B_LUPPERARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_LUPPERARM).setSize(0.090, 0.311, 0.090);
        getBodyPart(B_LUPPERARM).setMass(2.4);

        getBodyPart(B_RUPPERARM).setId(B_RUPPERARM);
        getBodyPart(B_RUPPERARM).setName("RightUpperArm");
        getBodyPart(B_RUPPERARM).setParentId(B_THORAX);
        getBodyPart(B_RUPPERARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_RUPPERARM).setSize(0.090, 0.311, 0.090);
        getBodyPart(B_RUPPERARM).setMass(2.4);

        getBodyPart(B_LFOREARM).setId(B_LFOREARM);
        getBodyPart(B_LFOREARM).setName("LeftForeArm");
        getBodyPart(B_LFOREARM).setParentId(B_LUPPERARM);
        getBodyPart(B_LFOREARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_LFOREARM).setSize(0.084, 0.284, 0.084);
        getBodyPart(B_LFOREARM).setMass(1.6);

        getBodyPart(B_RFOREARM).setId(B_RFOREARM);
        getBodyPart(B_RFOREARM).setName("RightForeArm");
        getBodyPart(B_RFOREARM).setParentId(B_RUPPERARM);
        getBodyPart(B_RFOREARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_RFOREARM).setSize(0.084, 0.284, 0.084);
        getBodyPart(B_RFOREARM).setMass(1.6);
    }

    void HalfHumanCreature::setConstraints( ) {
        for (int i=  0 ; i < CONSTRAINTS ;++i){
            getConstraint(i).setType(Constraint::CT_GENERIC_6DOF);
        }
        getConstraint(C_NECK).setId(C_NECK);
        getConstraint(C_NECK).setName("Neck");
        getConstraint(C_NECK).setIdBodyA(getBodyPart(B_HEAD).getId());
        getConstraint(C_NECK).setIdBodyB(getBodyPart(B_THORAX).getId());
        getConstraint(C_NECK).setConnectionA(Constraint::SPHERE_BOTTOM);
        getConstraint(C_NECK).setConnectionB(Constraint::BOX_TOP);
        getConstraint(C_NECK).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_NECK).setAngularLowerUpperLimits(
                btVector3(-M_PI_4 / 2.0, -M_PI_4 / 2.0, -M_PI_4 / 2.0),
                btVector3(M_PI_4 / 2.0, M_PI_4 / 2.0, M_PI_4 / 2.0));

        getConstraint(C_LSHOULDER).setId(C_LSHOULDER);
        getConstraint(C_LSHOULDER).setName("LeftShoulder");
        getConstraint(C_LSHOULDER).setIdBodyA(getBodyPart(B_THORAX).getId());
        getConstraint(C_LSHOULDER).setIdBodyB(getBodyPart(B_LUPPERARM).getId());
        getConstraint(C_LSHOULDER).setConnectionA(Constraint::BOX_TOP_LEFT);
        getConstraint(C_LSHOULDER).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_LSHOULDER).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LSHOULDER).setAngularLowerUpperLimits(
                //                                btVector3((2.0 / 3.0) * -M_PI_2, 0, -M_PI_2),
                //                                btVector3((2.0 / 3.0) * M_PI_2, M_PI_4, M_PI_2));
                btVector3(0.0, 0.0, 0.0),
                btVector3(0.0, -1.0, -1));

        getConstraint(C_RSHOULDER).setId(C_RSHOULDER);
        getConstraint(C_RSHOULDER).setName("RightShoulder");
        getConstraint(C_RSHOULDER).setIdBodyA(getBodyPart(B_THORAX).getId());
        getConstraint(C_RSHOULDER).setIdBodyB(getBodyPart(B_RUPPERARM).getId());
        getConstraint(C_RSHOULDER).setConnectionA(Constraint::BOX_TOP_RIGHT);
        getConstraint(C_RSHOULDER).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_RSHOULDER).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RSHOULDER).setAngularLowerUpperLimits(
//                btVector3((2.0 / 3.0) * -M_PI_2, 0, -M_PI_2),
//                btVector3((2.0 / 3.0) * M_PI_2, M_PI_4, M_PI_2));
                btVector3(0.0, 0.0, 0.0),
                btVector3(0.0, -1.0, -1));


        getConstraint(C_LELBOW).setId(C_LELBOW);
        getConstraint(C_LELBOW).setName("LeftElbow");
        getConstraint(C_LELBOW).setIdBodyA(getBodyPart(B_LUPPERARM).getId());
        getConstraint(C_LELBOW).setIdBodyB(getBodyPart(B_LFOREARM).getId());
        getConstraint(C_LELBOW).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_LELBOW).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_LELBOW).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LELBOW).setAngularLowerUpperLimits(
                btVector3(0, 0, 0),
                btVector3(0, 0, 0));
//                btVector3(M_PI_2, 0, 0));
        //        btVector3(M_PI_2 + M_PI_4, 0, 0));

        getConstraint(C_RELBOW).setId(C_RELBOW);
        getConstraint(C_RELBOW).setName("RightElbow");
        getConstraint(C_RELBOW).setIdBodyA(getBodyPart(B_RUPPERARM).getId());
        getConstraint(C_RELBOW).setIdBodyB(getBodyPart(B_RFOREARM).getId());
        getConstraint(C_RELBOW).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_RELBOW).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_RELBOW).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RELBOW).setAngularLowerUpperLimits(
                btVector3(0, 0, 0),
                btVector3(0, 0, 0));
        //                btVector3(M_PI_2 + M_PI_4, 0, 0));
    }
}
